#include <xrobot_control/wx_controller.h>


/**
 * @brief 打开串口，循环收发数据，与下位机进行通信
 */    
void JointsStatesController::start(void)
{
    // 打开串口
    Open_Serial();

    ros::Rate loop_rate(20);
    
    // 循环收发串口数据
    while (ros::ok())
    {
        // 更新数据
        UpdateData();

        // 延时等待
        loop_rate.sleep();

        // 查看中断函数
        ros::spinOnce();
    }

    // 程序关闭
    serial_port.close();
    ros::waitForShutdown();
}
int main(int argc, char* argv[])
{
    // 初始化节点
    ros::init(argc, argv, "joints_states_node");

    // 创建控制器
    JointsStatesController ctrl_node("xrobot", "/dev/TuTu_USB2");

    // 开启串口，进行数据收发
    ctrl_node.start();
    return 0;
} 
 
